#include "Integrator.h"
#include "Ode45.h"
#include "MatrixClasses.h"
#include "LinAlgDecl.h"

using namespace RSIM;

// This is the function which is to be integrated
// Example taken from page 170 of the book
// @book{hairer1993,
// title={Solving ordinary differential equations: Nonstiff problems},
// author={Hairer, E. and N{\o}rsett, S.P. and Wanner, G.},
// year={1993},
// publisher={Springer}
// }

// xdot = 1 + x^2*y -4x
// ydot = 3x - x^2*y
// initial state: x = 1.5, y = 3
// integrate from t = 0,20

void calcYDot(const double& t,
              const Vector& Y,
              Vector& YDot){
        
        // This is the input vector
        const double *y0 = Y.Ptr();
        
        // Create new labels
        const double& x = y0[0];
        const double& y = y0[1];
        
        // This is vector ydot
        double *y0dot = YDot.wPtr();        
        y0dot[0] = 1 + x*x*y -4*x ;
        y0dot[1] = 3*x - x*x*y;
}

int main(){
        Vector Y0;
                
        // Allocate space for 2 elements
        Y0.resize(2);
        
        // Define initial state
        // x0 = 1, y0 = 2
        Y0(0) = 1.5;
        Y0(1) = 3.0;
        
        // time
        const double& start = 0.0;
        const double& end = 20.0;
        
        Integrator *Integ = new Ode45;
        
        // Set integration Tolerances (Optional)
        Integ->setAbsTol(1.0E-7);
        Integ->setRelTol(1.0E-7);
        
        // Write state vector with a time stamp at each time step
        // 0 if no output is required
        const int& writeState = 1;
                
        Integ->stepTo(calcYDot,         // Function which calculates ydot
                      start,            // starting time
                      end,              // end time
                      Y0,               // initial state
                      writeState,       // 1 if state vector is to be written, 0 if no output is required from the integrator
                      "y"               // name of the file in which the output is written
                      );
        delete Integ;
        return 0;
}